#include"Func_def.h"
#include "WProgram.h"

/*enviornment variables*/
CANON_UNITS currUnit = CANON_UNITS_INCHES;
CANON_FEED_REFERENCE curreFeedRef = CANON_WORKPIECE;
CANON_PLANE currPlane = CANON_PLANE_XY;
CANON_MOTION_MODE currMotionMode = CANON_EXACT_STOP;
CANON_DIRECTION direction;
CANON_SPEED_FEED_MODE currSpeedFeedMode = CANON_INDEPENDENT;


CANON_PT Origin_X,Origin_Y,Origin_Z,Origin_A,Origin_B,Origin_C;
CANON_PT Cur_X,Cur_Y,Cur_Z,Cur_A,Cur_B,Cur_C;
int AXIS_SPEED[6];
int FEED_RATE = MAX_FEED_RATE;
int TRAVERSE_RATE = MAX_FEED_RATE;
int MACHINE_MOVING = 0;
int dir[6]={
  0,0,0,0,0,0};

CANON_SPINDLE_SPEED SPINDLE_SPEED;
axis_data data;

/*****/
//feed sync realted variables
int overflow_count = 0;
CANON_SPINDLE_SPEED dynamicSpindleSpeed = 1;

char ackChar = 'r';


int32 power10(int x)      //cant find negative powers of 10 as float cant be used
{
  int32 res = 1;    
  for(int i=0;i<x;i++)
    res = res * 10;
  return res;
}

/****************REPRESENTATION FUNCTIONS***************/

void SET_ORIGIN_OFFSETS(CANON_PT x, CANON_PT y, CANON_PT z, CANON_PT a, CANON_PT b, CANON_PT c)
{
  Origin_X = Cur_X = x;
  Origin_Y = Cur_Y = y;
  Origin_Z = Cur_Z = z;
  Origin_A = Cur_A = a;
  Origin_B = Cur_B = b;
  Origin_C = Cur_C = c; 
}

void SELECT_PLANE(CANON_PLANE plane)
{
  currPlane = plane;
}

void USE_LENGTH_UNITS(CANON_UNITS units)
{
  currUnit = units;
}

/****************FREE SPACE MOTION FUNCTIONS**********************/

void STRAIGHT_TRAVERSE(CANON_PT x, CANON_PT y, CANON_PT z, CANON_PT a, CANON_PT b, CANON_PT c)
{
  int tempFinalPoints[6],tempInitialPoints[6];
  int tempRate = FEED_RATE;
  SET_FEED_RATE(TRAVERSE_RATE);
  tempInitialPoints[0] = Cur_X; 
  tempInitialPoints[1] = Cur_Y; 
  tempInitialPoints[2] = Cur_Z; 
  tempInitialPoints[3] = Cur_A; 
  tempInitialPoints[4] = Cur_B; 
  tempInitialPoints[5] = Cur_C;
  tempFinalPoints[0] = x; 
  tempFinalPoints[1] = y; 
  tempFinalPoints[2] = z; 
  tempFinalPoints[3] = a; 
  tempFinalPoints[4] = b; 
  tempFinalPoints[5] = c;
  for(int i =0;i<6;i++)
  {
    if(tempInitialPoints[i]>=0 && tempFinalPoints[i]>tempInitialPoints[i])
      dir[i] = 1;
    else if(tempInitialPoints[i]>=0 && tempFinalPoints[i]<tempInitialPoints[i])
      dir[i] = -1;
    else if(tempInitialPoints[i]<0 && tempFinalPoints[i]<tempInitialPoints[i])
      dir[i] = 1;
    else if(tempInitialPoints[i]<0 && tempFinalPoints[i]>tempInitialPoints[i])
      dir[i] = -1;
    else
      dir[i] = 0;
  }
  ADD_BACKLASH();
  if(Cur_X!=x || Cur_Y!=y || Cur_Z!=z || Cur_A!=a || Cur_B!=b || Cur_C!=c)
  {
    bressenheim(Cur_X,Cur_Y,Cur_Z,Cur_A,Cur_B,Cur_C,x,y,z,a,b,c);
    Cur_X = x;
    Cur_Y = y;
    Cur_Z = z;
    Cur_A = a;
    Cur_B = b;
    Cur_C = c;
  }
  SET_FEED_RATE(tempRate);
}

/****************MACHINING ATTRIBUTES***************/

void SET_FEED_RATE(int rate)
{
    //SerialUSB.println("in set_feed_rate");
  if(rate>MAX_FEED_RATE)
    FEED_RATE = MAX_FEED_RATE;
  else
    FEED_RATE = rate;
    //SerialUSB.println(FEED_RATE);
}

void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference)
{
  curreFeedRef = reference;
}

void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode)
{
  currMotionMode = mode;
}

void START_SPEED_FEED_SYNCH ()
{
  currSpeedFeedMode = CANON_SYNCHED;
}

void STOP_SPEED_FEED_SYNCH ()
{
  currSpeedFeedMode = CANON_INDEPENDENT;
}

/*************MACHINING FUNCTIONS*********************/
//S-curve not implemented, speed inserted in fifo is the feedrate, n/T not inserted instead of feedrate
void ARC_FEED(CANON_PT first_end, CANON_PT second_end, CANON_PT first_axis, CANON_PT second_axis, CANON_PT rotation, CANON_PT axis_end_point, CANON_PT a, CANON_PT b, CANON_PT c)
{
/*
  cout<<"in arcFeed:\n";
  cout<<first_end;
 cout<<":";
 cout<<second_end;
cout<<":";
cout<<first_axis;
cout<<":";
cout<<second_axis;
cout<<":";
cout<<rotation;
cout<<":";
cout<<axis_end_point;
cout<<":";
cout<<a;
cout<<":";
cout<<b;
cout<<":";
cout<<c<<"\n";*/
  double p1,p2,p3;
  double pa = Cur_A,pb = Cur_B,pc = Cur_C;

  if(currPlane == CANON_PLANE_XY)
  {
    p1 = Cur_X;
    p2 = Cur_Y;
    p3 = Cur_Z;
  }
  else if(currPlane == CANON_PLANE_YZ)
  {
    p1 = Cur_Y;
    p2 = Cur_Z;
    p3 = Cur_X;
  }
  else if(currPlane == CANON_PLANE_XZ)
  {
    p1 = Cur_Z;
    p2 = Cur_X;
    p3 = Cur_Y;
  }

  int seg_len = 1;
  double theta1 = atan2((p2 - second_axis),(p1 - first_axis));
  double theta2 = atan2((second_end - second_axis),(first_end - first_axis));
  double deltaTheta = (theta2 - theta1);
  double r = sqrt(pow((first_axis - p1),2) +  pow((second_axis - p2),2));
/*
  cout<<"second end:");
  SerialUSB.println(atan2(4,0));
  cout<<":");
  SerialUSB.println(first_axis);

  cout<<":");
  SerialUSB.println(second_end);

    cout<<":");
  SerialUSB.println(first_end);

  */
/*
  cout<<"deltaTEhta:";
  cout<<deltaTheta<<"\n";

  cout<<"tEhta1:";
  cout<<theta1<<"\n";

  cout<<"tEhta2:";
  cout<<theta2<<"\n";
*/
  if(rotation == 0)
  {
    int pts = abs((axis_end_point - p3));
    int dp3 = ((axis_end_point - p3)/pts);
    int dpa = ((a - pa)/pts);
    int dpb = ((b - pb)/pts);
    int dpc = ((c - pc)/pts);
    double T = (double)pts/FEED_RATE;
//    cout<<pts<<":"<<dp3<<"\n";
    while(p3!=axis_end_point)
    {
      p3 = p3 + dp3;
      pa = pa + dpa;
      pb = pb + dpb;
      pc = pc + dpc;

      if(currPlane == CANON_PLANE_XY)
      {
        data.x = p1;
        data.y = p2;
        data.z = round(p3);
      }
      else if(currPlane == CANON_PLANE_YZ)
      {
        data.y = p1;
        data.z = p2;
        data.x = round(p3);
      }
      else if(currPlane == CANON_PLANE_XZ)
      {
        data.z = p1;
        data.x = p2;
        data.y = round(p3);
      }

      data.a = round(pa);
      data.b = round(pb);
      data.c = round(pc);

      data.Speed = round((double)pts/T);
      while(fifo_isfull())
      ;
      fifo_insert(data);
/*
      cout<<" data.x = ";
      cout<<data.x;
      cout<<" data.y = ";
      cout<<data.y;
      cout<<" data.z = ";
      cout<<data.z;
      cout<<" data.a = ";
      cout<<data.a;
      cout<<" data.b = ";
      cout<<data.b;
      cout<<" data.c = ";
      cout<<data.c;
      cout<<" data.Speed = ";
      cout<<data.Speed<<"\n";*/
    }

  }
  else if(rotation>0)
  {
//    cout<<"rot +ve\n";
    if(deltaTheta < 0)
        deltaTheta = PI - deltaTheta;
    deltaTheta = (rotation - 1)*2*PI + deltaTheta;
    int seg_num = (deltaTheta * r)/seg_len;
    double dTheta = deltaTheta / seg_num;
    double dp3 = (axis_end_point - p3)/seg_num;
    double dpa = (pa - a)/seg_num;
    double dpb = (pb - b)/seg_num;
    double dpc = (pc - c)/seg_num;
    double T = (r * deltaTheta)/FEED_RATE;
/*
    cout<<deltaTheta<<"\n";
    cout<<seg_num<<"\n";
    cout<<dTheta<<"\n";
    cout<<T<<"\n";
*/
    for(int i=0 ; i<seg_num ; i++)
    {
      p1 = first_axis + r*cos(theta1 + (i+1)*dTheta);
      p2 = second_axis + r*sin(theta1 + (i+1)*dTheta);
      p3 = p3 + dp3;
      pa = pa + dpa;
      pb = pb + dpb;
      pc = pc + dpc;

      if(currPlane == CANON_PLANE_XY)
      {
        data.x = round(p1);
        data.y = round(p2);
        data.z = round(p3);
      }
      else if(currPlane == CANON_PLANE_YZ)
      {
        data.y = round(p1);
        data.z = round(p2);
        data.x = round(p3);
      }
      else if(currPlane == CANON_PLANE_XZ)
      {
        data.z = round(p1);
        data.x = round(p2);
        data.y = round(p3);
      }
      data.a = round(pa);
      data.b = round(pb);
      data.c = round(pc);

      data.Speed = round(seg_num/T);
      while(fifo_isfull())
      ;
      fifo_insert(data);
/*
  cout<<" data.x = ";
  cout<<data.x;
  cout<<" data.y = ";
  cout<<data.y;
  cout<<" data.z = ";
  cout<<data.z;
  cout<<" data.a = ";
  cout<<data.a;
  cout<<" data.b = ";
  cout<<data.b;
  cout<<" data.c = ";
  cout<<data.c;
  cout<<" data.Speed = ";
  cout<<data.Speed<<"\n";
*/

    }
  }
  else
  {
  //  SerialUSB.println("in else");
    if(deltaTheta > 0)
        deltaTheta = -deltaTheta - PI;
    deltaTheta = (rotation + 1)*2*PI + (deltaTheta);
    int seg_num = (-deltaTheta * r)/seg_len;
    double dTheta = deltaTheta / seg_num;
    double dp3 = (axis_end_point - p3)/seg_num;
    double dpa = (pa - a)/seg_num;
    double dpb = (pb - b)/seg_num;
    double dpc = (pc - c)/seg_num;
    double T = (r * (-deltaTheta))/FEED_RATE;
    /*
    SerialUSB.println(deltaTheta);
    SerialUSB.println(seg_num);
    SerialUSB.println(dTheta);
    SerialUSB.println(T);
    */

    for(int i=0 ; i < seg_num ; i++)
    {
      p1 = first_axis + r*cos(theta1 + (i+1)*dTheta);
      p2 = second_axis + r*sin(theta1 + (i+1)*dTheta);
      p3 = p3 + dp3;
      pa = pa + dpa;
      pb = pb + dpb;
      pc = pc + dpc;

      if(currPlane == CANON_PLANE_XY)
      {
        data.x = round(p1);
        data.y = round(p2);
        data.z = round(p3);
      }
      else if(currPlane == CANON_PLANE_YZ)
      {
        data.y = round(p1);
        data.z = round(p2);
        data.x = round(p3);
      }
      else if(currPlane == CANON_PLANE_XZ)
      {
        data.x = round(p1);
        data.z = round(p2);
        data.y = round(p3);
      }
      data.a = round(pa);
      data.b = round(pb);
      data.c = round(pc);
      data.Speed = seg_num/T;
      while(fifo_isfull())
      ;
      fifo_insert(data);
/*
      cout<<" data.x = ";
      cout<<data.x;
      cout<<" data.y = ";
      cout<<data.y;
      cout<<" data.z = ";
      cout<<data.z;
      cout<<" data.a = ";
      cout<<data.a;
      cout<<" data.b = ";
      cout<<data.b;
      cout<<" data.c = ";
      cout<<data.c;
      cout<<" data.Speed = ";
      cout<<data.Speed<<"\n";
*/
    }
  }
  Cur_X = data.x;
  Cur_Y = data.y;
  Cur_Z = data.z;
  Cur_A = data.a;
  Cur_B = data.b;
  Cur_C = data.c;
}

void DWELL(int milliseconds)
{
  while(!fifo_isempty())  //wait till all the points in drive buffer are removed
    ;
  delay(milliseconds);  //might be incorrect as this delay might interfere with pulsingEngine implementation, refer to maple documentation
}

void STRAIGHT_FEED(CANON_PT x, CANON_PT y, CANON_PT z, CANON_PT a, CANON_PT b, CANON_PT c)
{
  //SerialUSB.println("in straight  feed");
  //SerialUSB.println(Cur_X);
  //SerialUSB.println(x);
  if(Cur_X!=x || Cur_Y!=y || Cur_Z!=z || Cur_A!=a || Cur_B!=b || Cur_C!=c)
  {
    //SerialUSB.println("if straight feed");
    bressenheim(Cur_X,Cur_Y,Cur_Z,Cur_A,Cur_B,Cur_C,x,y,z,a,b,c);
    Cur_X = x;
    Cur_Y = y;
    Cur_Z = z;
    Cur_A = a;
    Cur_B = b;
    Cur_C = c;
  }  
}

/**************SPINDLE FUNCTIONS**********************/

void SET_SPINDLE_SPEED(int speed)
{
  int SpindleSpeed;
  SpindleSpeed = 65536*(SPINDLE_SPEED - MIN_SPINDLE_SPEED)/(MAX_SPINDLE_SPEED - MIN_SPINDLE_SPEED);
  if(speed>MAX_SPINDLE_SPEED)
  {
    SPINDLE_SPEED = MAX_SPINDLE_SPEED;
  }
  SPINDLE_SPEED = speed;
  pwmWrite(SpindleSpeedPin,SpindleSpeed);
}

void START_SPINDLE_CLOCKWISE ()
{
  int SpindleSpeed;
  SpindleSpeed = 65536*(SPINDLE_SPEED - MIN_SPINDLE_SPEED)/(MAX_SPINDLE_SPEED - MIN_SPINDLE_SPEED);
  digitalWrite(Spindle1,1);
  digitalWrite(Spindle2,0);		// For clockwise motion dir = 1
  pwmWrite(SpindleSpeedPin,SpindleSpeed);
}

void START_SPINDLE_COUNTERCLOCKWISE ()
{
  int SpindleSpeed;
  SpindleSpeed = 65536*(SPINDLE_SPEED - MIN_SPINDLE_SPEED)/(MAX_SPINDLE_SPEED - MIN_SPINDLE_SPEED);
  digitalWrite(Spindle1,0);
  digitalWrite(Spindle2,1);		// For anticlockwise motion dir = 0
  pwmWrite(SpindleSpeedPin,SpindleSpeed);
}

void STOP_SPINDLE_TURNING()
{
  digitalWrite(Spindle1,0);
  digitalWrite(Spindle2,0);		
}

//not implemented yet
void ORIENT_SPINDLE(int orientation, CANON_DIRECTION direction)
{
}

void USE_NO_SPINDLE_FORCE()
{
}

void USE_NO_SPINDLE_TORQUE()
{
}

void USE_SPINDLE_FORCE(int force)
{
}

void USE_SPINDLE_TORQUE(int torque)
{
}

/**************TOOL FUNCTIONS*****************/

void USE_TOOL_LENGTH_OFFSET(int length)
{
  Tool_Offset = length;
}


void CHANGE_TOOL(int slot)
{
  Serial3.begin(serial3baud);
  Serial3.write(slot);  
  while(1)
  {
    if(Serial3.available() && Serial3.read()==ackChar)
      break;
    else
      Serial3.write(slot);  
  }
  Serial3.end();
}
/**************MISCELLANEOUS FUNCTIONS*********/

void FLOOD_OFF()
{
  digitalWrite(FloodPin,0);
}

void FLOOD_ON()
{
  digitalWrite(FloodPin,1);
}

void MIST_OFF()
{
  digitalWrite(MistPin,0);
}

void MIST_ON()
{
  digitalWrite(MistPin,1);
}

/*****************OTHER FUNCTIONS**************/

void bressenheim(CANON_PT s1,CANON_PT s2,CANON_PT s3,CANON_PT s4,CANON_PT s5,CANON_PT s6,CANON_PT e1,CANON_PT e2,CANON_PT e3,CANON_PT e4,CANON_PT e5,CANON_PT e6)
{
  //SerialUSB.println("in bres");
  int d1, d2, d3, d4, d5, d6;
  int Ad1, Ad2, Ad3, Ad4, Ad5, Ad6;
  int inc1, inc2, inc3, inc4, inc5, inc6;
  int d1x2, d2x2, d3x2, d4x2, d5x2, d6x2;
  int Dim1, Dim2, Dim3, Dim4, Dim5, Dim6;
  Dim1=s1;
  Dim2=s2;
  Dim3=s3;
  Dim4=s4;
  Dim5=s5;
  Dim6=s6;
  int Old1, Old2, Old3, Old4, Old5, Old6;
  Old1=s1;
  Old2=s2;
  Old3=s3;
  Old4=s4;
  Old5=s5;
  Old6=s6;
  int New1, New2, New3, New4, New5, New6;
  New1 = e1;
  New2 = e2;
  New3 = e3;
  New4 = e4;
  New5 = e5;
  New6 = e6;
  int err1, err2, err3, err4, err5, Cont3;
  int cont3;
  int64 square;
  int dist,rate[6];
  double Total_time;
  double t = 0,step_time;
  int i,dir[6];
  int max_speed = 0,curr_speed = 1;
  int TotalPoints[6],p = 0,b = 0,cnt = 0;
  float td,a;
  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";

  square = (e1-s1)*(e1-s1)+(e2-s2)*(e2-s2)+(e3-s3)*(e3-s3)+(e4-s4)*(e4-s4)+(e5-s5)*(e5-s5)+(e6-s6)*(e6-s6);
  dist = sqrt(square);
  //SerialUSB.println(square);
  //SerialUSB.println(dist);
  //printf("%d",dist);
  TotalPoints[0] = abs((e1-s1));
  TotalPoints[1] = abs((e2-s2));
  TotalPoints[2] = abs((e3-s3));
  TotalPoints[3] = abs((e4-s4));
  TotalPoints[4] = abs((e5-s5));
  TotalPoints[5] = abs((e6-s6));
  Total_time = float(dist)/FEED_RATE;
  //SerialUSB.println(FEED_RATE);
  AXIS_SPEED[0] = abs((e1-s1))/Total_time;
  AXIS_SPEED[1] = abs((e2-s2))/Total_time;
  AXIS_SPEED[2] = abs((e3-s3))/Total_time;
  AXIS_SPEED[3] = abs((e4-s4))/Total_time;
  AXIS_SPEED[4] = abs((e5-s5))/Total_time;
  AXIS_SPEED[5] = abs((e6-s6))/Total_time;
  //SerialUSB.println(Total_time);
  //SerialUSB.println(AXIS_SPEED[0]);
  //SerialUSB.println(AXIS_SPEED[1]);
  //SerialUSB.println(AXIS_SPEED[2]);
  //SerialUSB.println(AXIS_SPEED[3]);
  //SerialUSB.println(AXIS_SPEED[4]);
  //SerialUSB.println(AXIS_SPEED[5]);
  for(int i=0;i<6;i++)
  {
    if(AXIS_SPEED[i]>MAX_AXIS_SPEED[i])
    {
      AXIS_SPEED[i] = MAX_AXIS_SPEED[i];
    }
  }

  for(int i=0;i<6;i++)
  {
    if(AXIS_SPEED[i]>max_speed)
    {
      max_speed = AXIS_SPEED[i];
	  p = TotalPoints[i];
    }
  }

  a = exp(4*(Acceleration/(float)max_speed));
  td = -log(max_speed-1)/log(a);
  step_time = 1;
  //cout<<"step_time:"<<step_time;
  d1 = New1 - Old1;
  d2 = New2 - Old2;
  d3 = New3 - Old3;
  d4 = New4 - Old4;
  d5 = New5 - Old5;
  d6 = New6 - Old6;

  if(d1 < 0)
    inc1 = -1;
  else
    inc1 =  1;

  if(d2 < 0)
    inc2 = -1;
  else
    inc2 =  1;

  if(d3 < 0)
    inc3 = -1;
  else
    inc3 =  1;

  if(d4 < 0)
    inc4 = -1;
  else
    inc4 =  1;

  if(d5 < 0)
    inc5 = -1;
  else
    inc5 =  1;

  if(d6 < 0)
    inc6 = -1;
  else
    inc6 =  1;

  Ad1 = abs(d1);
  Ad2 = abs(d2);
  Ad3 = abs(d3);
  Ad4 = abs(d4);
  Ad5 = abs(d5);
  Ad6 = abs(d6);

  d1x2 = Ad1*2;
  d2x2 = Ad2*2;
  d3x2 = Ad3*2;
  d4x2 = Ad4*2;
  d5x2 = Ad5*2;
  d6x2 = Ad6*2;

  if((Ad1>= Ad2) && (Ad1>= Ad3) && (Ad1>= Ad4) && (Ad1>= Ad5) && (Ad1>= Ad6))
  {
    err1 = d2x2 - Ad1;
    err2 = d3x2 - Ad1;
    err3 = d4x2 - Ad1;
    err4 = d5x2 - Ad1;
    err5 = d6x2 - Ad1;

    for(cont3=1;cont3<=Ad1;cont3++)
    {
      if (err1 > 0)
      {
        Dim2+= inc2;
        err1 -= d1x2;
      }


      if (err2 > 0)
      {
        Dim3+= inc3;
        err2 -= d1x2;
      }

      if (err3 > 0)
      {
        Dim4+= inc4;
        err3 -= d1x2;
      }

      if (err4 > 0)
      {
        Dim5+= inc5;
        err4 -= d1x2;
      }

      if (err5 > 0)
      {
        Dim6+= inc6;
        err5 -= d1x2;
      }

      err1 += d2x2;
      err2 += d3x2;
      err3 += d4x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim1+= inc1;

      t = t + step_time;
      if((max_speed-curr_speed)>10 && cnt<(p-b))
	  {
        //cout<<"->"<<max_speed<<":"<<t<<":"<<a<<":"<<td<<"\n";
        curr_speed = Speed_Profile_Acceleration(max_speed,t,a,td);
		cnt = cnt+1;
		b = cnt;
	  }
	  else if(cnt>=(p-b) && (currMotionMode = CANON_EXACT_STOP))
	  {
		curr_speed = Speed_Profile_Deceleration(max_speed,t,a,td);
		cnt= cnt+1;
	  }
	  else
	  {
        curr_speed = max_speed;
	    cnt = cnt+1;
		t = 0;
	  }
	  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = curr_speed;
      //cout<<"max speed:"<<max_speed<<" Speed:"<<data.Speed<<"\n";
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }

  if( (Ad2> Ad1) && (Ad2>= Ad3) && (Ad2>= Ad4) && (Ad2>= Ad5) && (Ad2>= Ad6))
  {
    err1 = d1x2 - Ad2;
    err2 = d3x2 - Ad2;
    err3 = d4x2 - Ad2;
    err4 = d5x2 - Ad2;
    err5 = d6x2 - Ad2;

    for(cont3 = 1;cont3<=Ad2;cont3++)
    {
      if (err1 > 0)
      {
        Dim1+= inc1;
        err1 -= d2x2;
      }
      if (err2 > 0)
      {
        Dim3+= inc3;
        err2 -= d2x2;
      }

      if (err3 > 0)
      {
        Dim4+= inc4;
        err3 -= d2x2;
      }

      if (err4 > 0)
      {
        Dim5+= inc5;
        err4 -= d2x2;
      }

      if (err5 > 0)
      {
        Dim6+= inc6;
        err5 -= d2x2;
      }

      err1 += d1x2;
      err2 += d3x2;
      err3 += d4x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim2+= inc2;

      t = t + step_time;
      if((max_speed-curr_speed)>10 && cnt<(p-b))
	  {
        curr_speed = Speed_Profile_Acceleration(max_speed,t,a,td);
		cnt = cnt+1;
		b = cnt;
	  }
	  else if(cnt>=(p-b) && (currMotionMode = CANON_EXACT_STOP))
	  {
		curr_speed = Speed_Profile_Deceleration(max_speed,t,a,td);
		cnt= cnt+1;
	  }
	  else
	  {
        curr_speed = max_speed;
	    cnt = cnt+1;
		t = 0;
	  }
	  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = curr_speed;
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }

  if( (Ad3> Ad1) && (Ad3> Ad2) && (Ad3>= Ad4) && (Ad3>= Ad5) && (Ad3>= Ad6)  )
  {
    err1 = d2x2 - Ad3;
    err2 = d1x2 - Ad3;
    err3 = d4x2 - Ad3;
    err4 = d5x2 - Ad3;
    err5 = d6x2 - Ad3;

    for(cont3 = 1;cont3<=Ad3;cont3++)
    {
      if (err1 > 0)
      {
        Dim2+= inc2;
        err1 -= d3x2;
      }

      if (err2 > 0)
      {
        Dim1+= inc1;
        err2 -= d3x2;
      }

      if (err3 > 0)
      {
        Dim4+= inc4;
        err3 -= d3x2;
      }

      if (err4 > 0)
      {
        Dim5+= inc5;
        err4 -= d3x2;
      }

      if (err5 > 0)
      {
        Dim6+= inc6;
        err5 -= d3x2;
      }

      err1 += d2x2;
      err2 += d1x2;
      err3 += d4x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim3+= inc3;
      t = t + step_time;
      if((max_speed-curr_speed)>10 && cnt<(p-b))
	  {
        curr_speed = Speed_Profile_Acceleration(max_speed,t,a,td);
		cnt = cnt+1;
		b = cnt;
	  }
	  else if(cnt>=(p-b) && (currMotionMode = CANON_EXACT_STOP))
	  {
		curr_speed = Speed_Profile_Deceleration(max_speed,t,a,td);
		cnt= cnt+1;
	  }
	  else
	  {
        curr_speed = max_speed;
	    cnt = cnt+1;
		t = 0;
	  }
	  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = curr_speed;
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }
  if((Ad4> Ad1) && (Ad4> Ad2) && (Ad4> Ad3) && (Ad4>= Ad5) && (Ad4>= Ad6))
  {
    err1 = d1x2 - Ad4;
    err2 = d2x2 - Ad4;
    err3 = d3x2 - Ad4;
    err4 = d5x2 - Ad4;
    err5 = d6x2 - Ad4;

    for(cont3 = 1;cont3<=Ad4;cont3++)
    {
      if (err1 > 0)
      {
        Dim1+= inc1;
        err1 -= d4x2;
      }
      if (err2 > 0)
      {
        Dim2+= inc2;
        err2 -= d4x2;
      }

      if (err3 > 0)
      {
        Dim3+= inc3;
        err3 -= d4x2;
      }

      if (err4 > 0)
      {
        Dim5+= inc5;
        err4 -= d4x2;
      }

      if (err5 > 0)
      {
        Dim6+= inc6;
        err5 -= d4x2;
      }

      err1 += d1x2;
      err2 += d2x2;
      err3 += d3x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim4+= inc4;

      t = t + step_time;
      if((max_speed-curr_speed)>10 && cnt<(p-b))
	  {
        curr_speed = Speed_Profile_Acceleration(max_speed,t,a,td);
		cnt = cnt+1;
		b = cnt;
	  }
	  else if(cnt>=(p-b) && (currMotionMode = CANON_EXACT_STOP))
	  {
		curr_speed = Speed_Profile_Deceleration(max_speed,t,a,td);
		cnt= cnt+1;
	  }
	  else
	  {
        curr_speed = max_speed;
	    cnt = cnt+1;
		t = 0;
	  }
	  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = curr_speed;
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }

  if((Ad5> Ad1) && (Ad5> Ad2) && (Ad5> Ad3) && (Ad5> Ad4) && (Ad5>= Ad6))
  {
    err1 = d1x2 - Ad5;
    err2 = d2x2 - Ad5;
    err3 = d3x2 - Ad5;
    err4 = d4x2 - Ad5;
    err5 = d6x2 - Ad5;

    for(cont3 = 1;cont3<=Ad5;cont3++)
    {
      if (err1 > 0)
      {
        Dim1+= inc1;
        err1 -= d5x2;
      }

      if (err2 > 0)
      {
        Dim2+= inc2;
        err2 -= d5x2;
      }

      if (err3 > 0)
      {
        Dim3+= inc3;
        err3 -= d5x2;
      }

      if (err4 > 0)
      {
        Dim4+= inc4;
        err4 -= d5x2;
      }

      if (err5 > 0)
      {
        Dim6+= inc6;
        err5 -= d5x2;
      }

      err1 += d1x2;
      err2 += d2x2;
      err3 += d3x2;
      err4 += d4x2;
      err5 += d6x2;

      Dim5+= inc5;

      //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";

      t = t + step_time;
      if((max_speed-curr_speed)>10 && cnt<(p-b))
	  {
        curr_speed = Speed_Profile_Acceleration(max_speed,t,a,td);
		cnt = cnt+1;
		b = cnt;
	  }
	  else if(cnt>=(p-b) && (currMotionMode = CANON_EXACT_STOP))
	  {
		curr_speed = Speed_Profile_Deceleration(max_speed,t,a,td);
		cnt= cnt+1;
	  }
	  else
	  {
        curr_speed = max_speed;
	    cnt = cnt+1;
		t = 0;
	  }
	  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = curr_speed;
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }

  if( (Ad6> Ad1) && (Ad6> Ad2) && (Ad6> Ad3) && (Ad6> Ad4) && (Ad6> Ad5)  )
  {
    err1 = d1x2 - Ad6;
    err2 = d2x2 - Ad6;
    err3 = d3x2 - Ad6;
    err4 = d4x2 - Ad6;
    err5 = d5x2 - Ad6;

    for(cont3 = 1;cont3<=Ad6;cont3++)
    {
      if (err1 > 0)
      {
        Dim1+= inc1;
        err1 -= d6x2;
      }

      if (err2 > 0)
      {
        Dim2+= inc2;
        err2 -= d6x2;
      }

      if (err3 > 0)
      {
        Dim3+= inc3;
        err3 -= d6x2;
      }

      if (err4 > 0)
      {
        Dim4+= inc4;
        err4 -= d6x2;
      }

      if (err5 > 0)
      {
        Dim5+= inc5;
        err5 -= d6x2;
      }

      err1 += d1x2;
      err2 += d2x2;
      err3 += d3x2;
      err4 += d4x2;
      err5 += d5x2;

      Dim6+= inc6;

      t = t + step_time;
      if((max_speed-curr_speed)>10 && cnt<(p-b))
	  {
        curr_speed = Speed_Profile_Acceleration(max_speed,t,a,td);
		cnt = cnt+1;
		b = cnt;
	  }
	  else if(cnt>=(p-b) && (currMotionMode = CANON_EXACT_STOP))
	  {
		curr_speed = Speed_Profile_Deceleration(max_speed,t,a,td);
		cnt = cnt+1;
	  }
	  else
	  {
        curr_speed = max_speed;
	    cnt = cnt+1;
		t = 0;
	  }
	  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = curr_speed;
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
    //  fifo.show();
  }
  //	ReadBuffer();
}

int Speed_Profile_Acceleration(int AXIS_RATE,double t,float p,float td)
{

  int speed = 0,i = 0,j;
  double cur_c; //td is the time at which speed will be half of maximum rate
  cur_c = (-(t+td));
  speed = (double)AXIS_RATE * (1/(1+pow(p,cur_c)));
  if(speed == 0)
    speed = 1;
  return speed;
}

int Speed_Profile_Deceleration(int AXIS_RATE,double t,float p,float td)
{
  int speed = 0,i = 0,j;
  double cur_c; //td is the time at which speed will be half of maximum rate
  cur_c = (-(t+td));
  speed = (double)AXIS_RATE * (1-(1/(1+pow(p,cur_c))));
  if(speed == 0)
    speed = 1;
  return speed;
}

void ADD_BACKLASH()
{
  int i,temp;
  /*if(MACHINE_MOVING==0)
  {   
    for(i=0;i<6;i++)
    {
      Backlash[i] = Backlash[i]*dir[i];
    }
    temp = FEED_RATE;
    FEED_RATE = 0.2 * temp;
    bressenheim_backlash_drive(0,0,0,0,0,0,Backlash[0],Backlash[1],Backlash[2],Backlash[3],Backlash[4],Backlash[5]);
    FEED_RATE = temp;
    MACHINE_MOVING = 1;
  }
  else
  {*/   
    for(i=0;i<6;i++)
    {
      if(dir[i] == -1)
        Backlash[i] = Backlash[i] * dir[i];
      else
        Backlash[i] = 0;
    }
    temp = FEED_RATE;
    FEED_RATE = 0.2 * temp;
    bressenheim_backlash_drive(0,0,0,0,0,0,Backlash[0],Backlash[1],Backlash[2],Backlash[3],Backlash[4],Backlash[5]);
    FEED_RATE = temp;
    MACHINE_MOVING = 1;
  //}
}

void bressenheim_backlash_drive(int s1,int s2,int s3,int s4,int s5,int s6,int e1,int e2,int e3,int e4,int e5,int e6)
{
  int d1, d2, d3, d4, d5, d6;
  int Ad1, Ad2, Ad3, Ad4, Ad5, Ad6;
  int inc1, inc2, inc3, inc4, inc5, inc6;
  int d1x2, d2x2, d3x2, d4x2, d5x2, d6x2;
  int Dim1, Dim2, Dim3, Dim4, Dim5, Dim6;
  Dim1=s1;
  Dim2=s2;
  Dim3=s3;
  Dim4=s4;
  Dim5=s5;
  Dim6=s6;
  int Old1, Old2, Old3, Old4, Old5, Old6;
  Old1=s1;
  Old2=s2;
  Old3=s3;
  Old4=s4;
  Old5=s5;
  Old6=s6;
  int New1, New2, New3, New4, New5, New6;
  New1 = e1; 
  New2 = e2; 
  New3 = e3; 
  New4 = e4; 
  New5 = e5; 
  New6 = e6;
  int err1, err2, err3, err4, err5, Cont3;
  int cont3;
  int square,dist,rate[6];
  double Total_time;
  double t=0,step_time = 0.5;
  int i;
  int max_speed=0,curr_speed=1;
  int TotalPoints[6],p=0,b=0,cnt=0;

  //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";

  square = (e1-s1)*(e1-s1)+(e2-s2)*(e2-s2)+(e3-s3)*(e3-s3)+(e4-s4)*(e4-s4)+(e5-s5)*(e5-s5)+(e6-s6)*(e6-s6);
  dist = sqrt(square);
  //printf("%d",dist);
  TotalPoints[0] = abs((e1-s1));
  TotalPoints[1] = abs((e2-s2));
  TotalPoints[2] = abs((e3-s3));
  TotalPoints[3] = abs((e4-s4));
  TotalPoints[4] = abs((e5-s5));
  TotalPoints[5] = abs((e6-s6));
  Total_time = float(dist)/FEED_RATE;
  AXIS_SPEED[0] = TotalPoints[0]/Total_time;
  AXIS_SPEED[1] = TotalPoints[1]/Total_time;
  AXIS_SPEED[2] = TotalPoints[2]/Total_time;
  AXIS_SPEED[3] = TotalPoints[3]/Total_time;
  AXIS_SPEED[4] = TotalPoints[4]/Total_time;
  AXIS_SPEED[5] = TotalPoints[5]/Total_time;
  for(int i=0;i<6;i++)
  {
    if(AXIS_SPEED[i]>MAX_AXIS_SPEED[i])
    {
      AXIS_SPEED[i] = MAX_AXIS_SPEED[i];
    }
  }

  for(int i=0;i<6;i++)
  {
    if(AXIS_SPEED[i]>max_speed)
    {
      max_speed = AXIS_SPEED[i];
      p = TotalPoints[i];
    }
  }

  d1 = New1 - Old1;
  d2 = New2 - Old2;
  d3 = New3 - Old3;
  d4 = New4 - Old4;
  d5 = New5 - Old5;
  d6 = New6 - Old6;

  if(d1 < 0) 
    inc1 = -1;
  else
    inc1 =  1;

  if(d2 < 0)
    inc2 = -1;
  else 
    inc2 =  1;

  if(d3 < 0)
    inc3 = -1;
  else
    inc3 =  1;

  if(d4 < 0)
    inc4 = -1;
  else
    inc4 =  1;

  if(d5 < 0)
    inc5 = -1;
  else
    inc5 =  1;

  if(d6 < 0)
    inc6 = -1;
  else
    inc6 =  1;

  Ad1 = abs(d1);
  Ad2 = abs(d2);
  Ad3 = abs(d3);
  Ad4 = abs(d4);
  Ad5 = abs(d5);
  Ad6 = abs(d6);

  d1x2 = Ad1*2;
  d2x2 = Ad2*2;
  d3x2 = Ad3*2;
  d4x2 = Ad4*2;
  d5x2 = Ad5*2;
  d6x2 = Ad6*2;

  if((Ad1>= Ad2) && (Ad1>= Ad3) && (Ad1>= Ad4) && (Ad1>= Ad5) && (Ad1>= Ad6))
  {
    err1 = d2x2 - Ad1;
    err2 = d3x2 - Ad1;
    err3 = d4x2 - Ad1;
    err4 = d5x2 - Ad1;
    err5 = d6x2 - Ad1;

    for(cont3=1;cont3<=Ad1;cont3++)
    {			
      if (err1 > 0)
      {
        Dim2+= inc2;
        err1 -= d1x2;
      }


      if (err2 > 0)
      {
        Dim3+= inc3;
        err2 -= d1x2;
      }

      if (err3 > 0)
      {
        Dim4+= inc4;
        err3 -= d1x2;
      }

      if (err4 > 0)
      {
        Dim5+= inc5;
        err4 -= d1x2;
      }

      if (err5 > 0)
      {
        Dim6+= inc6;
        err5 -= d1x2;
      }

      err1 += d2x2;
      err2 += d3x2;
      err3 += d4x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim1+= inc1; 

      //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = FEED_RATE;          
      while(fifo_isfull())
        ;
      fifo_insert(data);	
    }
  }

  if( (Ad2> Ad1) && (Ad2>= Ad3) && (Ad2>= Ad4) && (Ad2>= Ad5) && (Ad2>= Ad6))
  {			
    err1 = d1x2 - Ad2;
    err2 = d3x2 - Ad2;
    err3 = d4x2 - Ad2;
    err4 = d5x2 - Ad2;
    err5 = d6x2 - Ad2;

    for(cont3 = 1;cont3<=Ad2;cont3++)
    {
      if (err1 > 0)
      {
        Dim1+= inc1;
        err1 -= d2x2;
      }
      if (err2 > 0)
      {
        Dim3+= inc3;
        err2 -= d2x2;
      }

      if (err3 > 0)  
      {
        Dim4+= inc4;
        err3 -= d2x2;
      }

      if (err4 > 0)  
      {
        Dim5+= inc5;
        err4 -= d2x2;
      }

      if (err5 > 0)  
      {
        Dim6+= inc6;
        err5 -= d2x2;
      }

      err1 += d1x2;
      err2 += d3x2;
      err3 += d4x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim2+= inc2;

      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = FEED_RATE;          
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }

  if( (Ad3> Ad1) && (Ad3> Ad2) && (Ad3>= Ad4) && (Ad3>= Ad5) && (Ad3>= Ad6)  )
  {	
    err1 = d2x2 - Ad3;
    err2 = d1x2 - Ad3;
    err3 = d4x2 - Ad3;
    err4 = d5x2 - Ad3;
    err5 = d6x2 - Ad3;

    for(cont3 = 1;cont3<=Ad3;cont3++)
    {
      if (err1 > 0) 
      {
        Dim2+= inc2;
        err1 -= d3x2;
      }

      if (err2 > 0)  
      {
        Dim1+= inc1;
        err2 -= d3x2;
      }

      if (err3 > 0)  
      {
        Dim4+= inc4;
        err3 -= d3x2;
      }

      if (err4 > 0)  
      {
        Dim5+= inc5;
        err4 -= d3x2;
      }

      if (err5 > 0)  
      {  
        Dim6+= inc6;
        err5 -= d3x2;
      }

      err1 += d2x2;
      err2 += d1x2;
      err3 += d4x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim3+= inc3;
      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = FEED_RATE;          
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }
  if((Ad4> Ad1) && (Ad4> Ad2) && (Ad4> Ad3) && (Ad4>= Ad5) && (Ad4>= Ad6))
  {
    err1 = d1x2 - Ad4;
    err2 = d2x2 - Ad4;
    err3 = d3x2 - Ad4;
    err4 = d5x2 - Ad4;
    err5 = d6x2 - Ad4;

    for(cont3 = 1;cont3<=Ad4;cont3++)
    {
      if (err1 > 0) 
      {
        Dim1+= inc1;
        err1 -= d4x2;
      }
      if (err2 > 0)  
      {
        Dim2+= inc2;
        err2 -= d4x2;
      }

      if (err3 > 0)  
      {
        Dim3+= inc3;
        err3 -= d4x2;
      }

      if (err4 > 0)  
      {
        Dim5+= inc5;
        err4 -= d4x2;
      }

      if (err5 > 0)  
      {
        Dim6+= inc6;
        err5 -= d4x2;
      }

      err1 += d1x2;
      err2 += d2x2;
      err3 += d3x2;
      err4 += d5x2;
      err5 += d6x2;

      Dim4+= inc4;

      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = FEED_RATE;          
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }

  if((Ad5> Ad1) && (Ad5> Ad2) && (Ad5> Ad3) && (Ad5> Ad4) && (Ad5>= Ad6))
  {
    err1 = d1x2 - Ad5;
    err2 = d2x2 - Ad5;
    err3 = d3x2 - Ad5;
    err4 = d4x2 - Ad5;
    err5 = d6x2 - Ad5;

    for(cont3 = 1;cont3<=Ad5;cont3++)
    {   
      if (err1 > 0) 
      {
        Dim1+= inc1;
        err1 -= d5x2;
      }

      if (err2 > 0)  
      {
        Dim2+= inc2;
        err2 -= d5x2;
      }

      if (err3 > 0)  
      {
        Dim3+= inc3;
        err3 -= d5x2;
      }

      if (err4 > 0)  
      {
        Dim4+= inc4;
        err4 -= d5x2;
      }

      if (err5 > 0)  
      {
        Dim6+= inc6;
        err5 -= d5x2;
      }

      err1 += d1x2;
      err2 += d2x2;
      err3 += d3x2;
      err4 += d4x2;
      err5 += d6x2;

      Dim5+= inc5;

      //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";

      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = FEED_RATE;          
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
  }		

  if( (Ad6> Ad1) && (Ad6> Ad2) && (Ad6> Ad3) && (Ad6> Ad4) && (Ad6> Ad5)  )
  {	
    err1 = d1x2 - Ad6;
    err2 = d2x2 - Ad6;
    err3 = d3x2 - Ad6;
    err4 = d4x2 - Ad6;
    err5 = d5x2 - Ad6;

    for(cont3 = 1;cont3<=Ad6;cont3++)
    {
      if (err1 > 0) 
      {
        Dim1+= inc1;
        err1 -= d6x2;
      }

      if (err2 > 0)  
      {
        Dim2+= inc2;
        err2 -= d6x2;
      }

      if (err3 > 0)  
      {
        Dim3+= inc3;
        err3 -= d6x2;
      }

      if (err4 > 0)  
      {
        Dim4+= inc4;
        err4 -= d6x2;
      }

      if (err5 > 0)  
      {
        Dim5+= inc5;
        err5 -= d6x2;
      }

      err1 += d1x2;
      err2 += d2x2;
      err3 += d3x2;
      err4 += d4x2;
      err5 += d5x2;

      Dim6+= inc6;

      data.x = Dim1, data.y = Dim2, data.z = Dim3;
      data.a = Dim4, data.b = Dim5, data.c = Dim6;
      data.Speed = FEED_RATE;          
      while(fifo_isfull())
        ;
      fifo_insert(data);
    }
    //  fifo.show();
  }
  //	ReadBuffer();
}


// Bressenheim Functions
/*
void bressenheim(CANON_PT s1,CANON_PT s2,CANON_PT s3,CANON_PT s4,CANON_PT s5,CANON_PT s6,CANON_PT e1,CANON_PT e2,CANON_PT e3,CANON_PT e4,CANON_PT e5,CANON_PT e6)
 {
 //SerialUSB.println("in bres");
 int d1, d2, d3, d4, d5, d6;
 int Ad1, Ad2, Ad3, Ad4, Ad5, Ad6;
 int inc1, inc2, inc3, inc4, inc5, inc6;
 int d1x2, d2x2, d3x2, d4x2, d5x2, d6x2;
 int Dim1, Dim2, Dim3, Dim4, Dim5, Dim6;
 Dim1=s1;
 Dim2=s2;
 Dim3=s3;
 Dim4=s4;
 Dim5=s5;
 Dim6=s6;
 int Old1, Old2, Old3, Old4, Old5, Old6;
 Old1=s1;
 Old2=s2;
 Old3=s3;
 Old4=s4;
 Old5=s5;
 Old6=s6;
 int New1, New2, New3, New4, New5, New6;
 New1 = e1; 
 New2 = e2; 
 New3 = e3; 
 New4 = e4; 
 New5 = e5; 
 New6 = e6;
 int err1, err2, err3, err4, err5, Cont3;
 int cont3;
 int square,dist,rate[6];
 double Total_time;
 double t=0,step_time = 0.5;
 int i,dir[6];
 int max_speed,curr_speed=1;
 
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 
 square = (e1-s1)*(e1-s1)+(e2-s2)*(e2-s2)+(e3-s3)*(e3-s3)+(e4-s4)*(e4-s4)+(e5-s5)*(e5-s5)+(e6-s6)*(e6-s6);
 dist = SquareRoot(square);
 //printf("%d",dist);
 Total_time = float(dist)/FEED_RATE;
 AXIS_SPEED[0] = abs((e1-s1))/Total_time;
 AXIS_SPEED[1] = abs((e2-s2))/Total_time;
 AXIS_SPEED[2] = abs((e3-s3))/Total_time;
 AXIS_SPEED[3] = abs((e4-s4))/Total_time;
 AXIS_SPEED[4] = abs((e5-s5))/Total_time;
 AXIS_SPEED[5] = abs((e6-s6))/Total_time;
 for(int i=0;i<6;i++)
 {
 if(AXIS_SPEED[i]>MAX_AXIS_SPEED[i])
 {
 AXIS_SPEED[i] = MAX_AXIS_SPEED[i];
 }
 }
 
 max_speed= AXIS_SPEED[0];
 for(int i=1;i<6;i++)
 {
 if(AXIS_SPEED[i]>max_speed)
 {
 max_speed = AXIS_SPEED[i];
 }
 }
 
 d1 = New1 - Old1;
 d2 = New2 - Old2;
 d3 = New3 - Old3;
 d4 = New4 - Old4;
 d5 = New5 - Old5;
 d6 = New6 - Old6;
 
 if(d1 < 0) 
 inc1 = -1;
 else
 inc1 =  1;
 
 if(d2 < 0)
 inc2 = -1;
 else 
 inc2 =  1;
 
 if(d3 < 0)
 inc3 = -1;
 else
 inc3 =  1;
 
 if(d4 < 0)
 inc4 = -1;
 else
 inc4 =  1;
 
 if(d5 < 0)
 inc5 = -1;
 else
 inc5 =  1;
 
 if(d6 < 0)
 inc6 = -1;
 else
 inc6 =  1;
 
 Ad1 = abs(d1);
 Ad2 = abs(d2);
 Ad3 = abs(d3);
 Ad4 = abs(d4);
 Ad5 = abs(d5);
 Ad6 = abs(d6);
 
 d1x2 = Ad1*2;
 d2x2 = Ad2*2;
 d3x2 = Ad3*2;
 d4x2 = Ad4*2;
 d5x2 = Ad5*2;
 d6x2 = Ad6*2;
 
 if((Ad1>= Ad2) && (Ad1>= Ad3) && (Ad1>= Ad4) && (Ad1>= Ad5) && (Ad1>= Ad6))
 {
 err1 = d2x2 - Ad1;
 err2 = d3x2 - Ad1;
 err3 = d4x2 - Ad1;
 err4 = d5x2 - Ad1;
 err5 = d6x2 - Ad1;
 
 for(cont3=1;cont3<=Ad1;cont3++)
 {			
 if (err1 > 0)
 {
 Dim2+= inc2;
 err1 -= d1x2;
 }
 
 
 if (err2 > 0)
 {
 Dim3+= inc3;
 err2 -= d1x2;
 }
 
 if (err3 > 0)
 {
 Dim4+= inc4;
 err3 -= d1x2;
 }
 
 if (err4 > 0)
 {
 Dim5+= inc5;
 err4 -= d1x2;
 }
 
 if (err5 > 0)
 {
 Dim6+= inc6;
 err5 -= d1x2;
 }
 
 err1 += d2x2;
 err2 += d3x2;
 err3 += d4x2;
 err4 += d5x2;
 err5 += d6x2;
 
 Dim1+= inc1; 
 
 t = t + step_time;
 if((max_speed-curr_speed)>10)
 curr_speed = SpeedProfile(max_speed,t);
 else
 curr_speed = max_speed;                  
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 data.x = Dim1, data.y = Dim2, data.z = Dim3;
 data.a = Dim4, data.b = Dim5, data.c = Dim6;
 data.Speed = curr_speed;          
 while(fifo_isfull())
 ;
 fifo_insert(data);	
 }
 }
 
 if( (Ad2> Ad1) && (Ad2>= Ad3) && (Ad2>= Ad4) && (Ad2>= Ad5) && (Ad2>= Ad6))
 {			
 err1 = d1x2 - Ad2;
 err2 = d3x2 - Ad2;
 err3 = d4x2 - Ad2;
 err4 = d5x2 - Ad2;
 err5 = d6x2 - Ad2;
 
 for(cont3 = 1;cont3<=Ad2;cont3++)
 {
 if (err1 > 0)
 {
 Dim1+= inc1;
 err1 -= d2x2;
 }
 if (err2 > 0)
 {
 Dim3+= inc3;
 err2 -= d2x2;
 }
 
 if (err3 > 0)  
 {
 Dim4+= inc4;
 err3 -= d2x2;
 }
 
 if (err4 > 0)  
 {
 Dim5+= inc5;
 err4 -= d2x2;
 }
 
 if (err5 > 0)  
 {
 Dim6+= inc6;
 err5 -= d2x2;
 }
 
 err1 += d1x2;
 err2 += d3x2;
 err3 += d4x2;
 err4 += d5x2;
 err5 += d6x2;
 
 Dim2+= inc2;
 
 t = t + step_time;
 
 if((max_speed-curr_speed)>10)
 curr_speed = SpeedProfile(max_speed,t);
 else
 curr_speed = max_speed;
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 data.x = Dim1, data.y = Dim2, data.z = Dim3;
 data.a = Dim4, data.b = Dim5, data.c = Dim6;
 data.Speed = curr_speed;
 while(fifo_isfull())
 ;
 fifo_insert(data);
 }
 }
 
 if( (Ad3> Ad1) && (Ad3> Ad2) && (Ad3>= Ad4) && (Ad3>= Ad5) && (Ad3>= Ad6)  )
 {	
 err1 = d2x2 - Ad3;
 err2 = d1x2 - Ad3;
 err3 = d4x2 - Ad3;
 err4 = d5x2 - Ad3;
 err5 = d6x2 - Ad3;
 
 for(cont3 = 1;cont3<=Ad3;cont3++)
 {
 if (err1 > 0) 
 {
 Dim2+= inc2;
 err1 -= d3x2;
 }
 
 if (err2 > 0)  
 {
 Dim1+= inc1;
 err2 -= d3x2;
 }
 
 if (err3 > 0)  
 {
 Dim4+= inc4;
 err3 -= d3x2;
 }
 
 if (err4 > 0)  
 {
 Dim5+= inc5;
 err4 -= d3x2;
 }
 
 if (err5 > 0)  
 {  
 Dim6+= inc6;
 err5 -= d3x2;
 }
 
 err1 += d2x2;
 err2 += d1x2;
 err3 += d4x2;
 err4 += d5x2;
 err5 += d6x2;
 
 Dim3+= inc3;
 t = t + step_time;
 
 if((max_speed-curr_speed)>10)
 curr_speed = SpeedProfile(max_speed,t);
 else
 curr_speed = max_speed;
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 data.x = Dim1, data.y = Dim2, data.z = Dim3;
 data.a = Dim4, data.b = Dim5, data.c = Dim6;
 data.Speed = curr_speed;
 while(fifo_isfull())
 ;                   
 fifo_insert(data);
 }
 }
 if((Ad4> Ad1) && (Ad4> Ad2) && (Ad4> Ad3) && (Ad4>= Ad5) && (Ad4>= Ad6))
 {
 err1 = d1x2 - Ad4;
 err2 = d2x2 - Ad4;
 err3 = d3x2 - Ad4;
 err4 = d5x2 - Ad4;
 err5 = d6x2 - Ad4;
 
 for(cont3 = 1;cont3<=Ad4;cont3++)
 {
 if (err1 > 0) 
 {
 Dim1+= inc1;
 err1 -= d4x2;
 }
 if (err2 > 0)  
 {
 Dim2+= inc2;
 err2 -= d4x2;
 }
 
 if (err3 > 0)  
 {
 Dim3+= inc3;
 err3 -= d4x2;
 }
 
 if (err4 > 0)  
 {
 Dim5+= inc5;
 err4 -= d4x2;
 }
 
 if (err5 > 0)  
 {
 Dim6+= inc6;
 err5 -= d4x2;
 }
 
 err1 += d1x2;
 err2 += d2x2;
 err3 += d3x2;
 err4 += d5x2;
 err5 += d6x2;
 
 Dim4+= inc4;
 
 t = t + step_time;
 
 if((max_speed-curr_speed)>10)
 curr_speed = SpeedProfile(max_speed,t);
 else
 curr_speed = max_speed;
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 data.x = Dim1, data.y = Dim2, data.z = Dim3;
 data.a = Dim4, data.b = Dim5, data.c = Dim6;
 data.Speed = curr_speed;
 while(fifo_isfull())
 ;
 fifo_insert(data);
 }
 }
 
 if((Ad5> Ad1) && (Ad5> Ad2) && (Ad5> Ad3) && (Ad5> Ad4) && (Ad5>= Ad6))
 {
 err1 = d1x2 - Ad5;
 err2 = d2x2 - Ad5;
 err3 = d3x2 - Ad5;
 err4 = d4x2 - Ad5;
 err5 = d6x2 - Ad5;
 
 for(cont3 = 1;cont3<=Ad5;cont3++)
 {   
 if (err1 > 0) 
 {
 Dim1+= inc1;
 err1 -= d5x2;
 }
 
 if (err2 > 0)  
 {
 Dim2+= inc2;
 err2 -= d5x2;
 }
 
 if (err3 > 0)  
 {
 Dim3+= inc3;
 err3 -= d5x2;
 }
 
 if (err4 > 0)  
 {
 Dim4+= inc4;
 err4 -= d5x2;
 }
 
 if (err5 > 0)  
 {
 Dim6+= inc6;
 err5 -= d5x2;
 }
 
 err1 += d1x2;
 err2 += d2x2;
 err3 += d3x2;
 err4 += d4x2;
 err5 += d6x2;
 
 Dim5+= inc5;
 
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 
 t = t + step_time;
 
 if((max_speed-curr_speed)>10)
 curr_speed = SpeedProfile(max_speed,t);
 else
 curr_speed = max_speed;
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 data.x = Dim1, data.y = Dim2, data.z = Dim3;
 data.a = Dim4, data.b = Dim5, data.c = Dim6;
 data.Speed = curr_speed;
 while(fifo_isfull())
 ;
 fifo_insert(data);
 }
 }		
 
 if( (Ad6> Ad1) && (Ad6> Ad2) && (Ad6> Ad3) && (Ad6> Ad4) && (Ad6> Ad5)  )
 {	
 err1 = d1x2 - Ad6;
 err2 = d2x2 - Ad6;
 err3 = d3x2 - Ad6;
 err4 = d4x2 - Ad6;
 err5 = d5x2 - Ad6;
 
 for(cont3 = 1;cont3<=Ad6;cont3++)
 {
 if (err1 > 0) 
 {
 Dim1+= inc1;
 err1 -= d6x2;
 }
 
 if (err2 > 0)  
 {
 Dim2+= inc2;
 err2 -= d6x2;
 }
 
 if (err3 > 0)  
 {
 Dim3+= inc3;
 err3 -= d6x2;
 }
 
 if (err4 > 0)  
 {
 Dim4+= inc4;
 err4 -= d6x2;
 }
 
 if (err5 > 0)  
 {
 Dim5+= inc5;
 err5 -= d6x2;
 }
 
 err1 += d1x2;
 err2 += d2x2;
 err3 += d3x2;
 err4 += d4x2;
 err5 += d5x2;
 
 Dim6+= inc6;
 
 t = t + step_time;
 if((max_speed-curr_speed)>10)
 curr_speed = SpeedProfile(max_speed,t);
 else
 curr_speed = max_speed;
 //cout<<Dim1<<" "<<Dim2<<" "<<Dim3<<" "<<Dim4<<" "<<Dim5<<" "<<Dim6<<"\n";
 data.x = Dim1, data.y = Dim2, data.z = Dim3;
 data.a = Dim4, data.b = Dim5, data.c = Dim6;
 data.Speed = curr_speed;
 while(fifo_isfull())
 ;
 fifo_insert(data);
 }
 //  fifo.show();
 }
 //	ReadBuffer();
 }
 
 int SpeedProfile(int AXIS_RATE,double t)
 {
 double speed;	
 int i=0,j; 
 double p=3,cur_c; //td is the time at which speed will be half of maximum rate     
 double td = ((double)AXIS_RATE)/(2*AccelerationSlope*power10(3));
 cur_c = (-(t-td));
 speed = AXIS_RATE * (1/(1+pow(p,cur_c)));
 if(speed<1)
 speed = 1;
 return speed;
 }
 */

//look for bugs here while debugging
int32 SquareRoot(int64 num) 
{
  int64 res = 0;
  int64 Bit = 1 << (sizeof(int64)*8 - 2); // The second-to-top bit is set: 1L<<30 for long

  // "bit" starts at the highest power of four <= the argument.
  while (Bit > num)
    Bit >>= 2;

  while (Bit != 0) {
    if (num >= res + Bit) {
      num -= res + Bit;
      res = (res >> 1) + Bit;
    }
    else
      res >>= 1;
    Bit >>= 2;
  }
  return res;
}

/****/
//function to implement feed sync 
void start_sync_timer()
{
  uint32 load_time = 1000;  //loaded to a time of 1 sec
  uint32 overflow;
  Timer3.setMode(1,TIMER_OUTPUT_COMPARE);
  overflow = Timer3.setPeriod(load_time);
  Timer3.setCompare(1,overflow);
  Timer3.attachInterrupt(1,handler_overflow);   
  Timer3.refresh();
  Timer3.resume();
  overflow_count = 0;
}

void handler_overflow()
{
  overflow_count++;
}

void stop_sync_timer()
{
  Timer3.setMode(1,TIMER_DISABLED);
}

void handler_spindle_interrupt_hi()
{
  
}

void handler_spindle_interrupt()
{
  uint16 count;
  uint32 load_time = 1000;  //loaded to a time of 1 sec
  uint16 overflow;
  uint64 time_elapsed = 0;
  Timer3.pause();
  count = Timer3.getCount();
  overflow = Timer3.getOverflow(); 
    
  time_elapsed = (overflow_count*load_time) + (load_time * count)/overflow  ;  //time elapsed for one spindle revolution in uSec
  dynamicSpindleSpeed = (1000)*(1000)*(1000)/(time_elapsed * 60);  //milli rpm
  
  SerialUSB.print("overflows ");
  SerialUSB.print(overflow_count);
  SerialUSB.print(" time ");
  SerialUSB.print(time_elapsed);
  SerialUSB.print(" dynamicspeed ");
  SerialUSB.println(dynamicSpindleSpeed);
  
  overflow_count = 0;
  Timer3.refresh();
  Timer3.resume();
}
